/*
Copyright (c) [2019年5月1日] [吴超]
[MBT_studio] is licensed under Mulan PSL v2.
You can use this software according to the terms and conditions of the Mulan PSL v2.
You may obtain a copy of Mulan PSL v2 at:
		 http://license.coscl.org.cn/MulanPSL2
THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
See the Mulan PSL v2 for more details.
*/
#include "stdafx.h"
#include "phy_Mujoco.h"

#include "core/引擎配置.h"
#include "物体/物体.h"
#include "面/面.h"

#include <matXX.h>
#include <tuple>

//#pragma comment(lib,"D:/U/DEV/mujoco/lib/mujoco.lib")



C_StrKey分配器	S_PhysicsMujoco::g_名称生成器;




#ifdef ADDRESS_SANITIZER
#include <sanitizer/asan_interface.h>
#elif defined(_MSC_VER)
#define ASAN_POISON_MEMORY_REGION(addr, size)
#define ASAN_UNPOISON_MEMORY_REGION(addr, size)
#else
#define ASAN_POISON_MEMORY_REGION(addr, size) ((void)(addr), (void)(size))
#define ASAN_UNPOISON_MEMORY_REGION(addr, size) ((void)(addr), (void)(size))
#endif

#define PTRDIFF(x, y) ((void*)(x) - (void*)(y))


// set statistics to default values; compute later in compiler
void mj_defaultStatistic(mjStatistic* stat) {
	mju_zero3(stat->center);
	stat->extent = 2;
	stat->meaninertia = 1;
	stat->meanmass = 1;
	stat->meansize = 0.2;
}


// number of bytes to be skipped to achieve 64-byte alignment
static unsigned int SKIP(intptr_t offset) {
	const unsigned int align = 64;
	// compute skipped bytes
	return (align - (offset % align)) % align;
}





// set pointers in mjModel buffer
static void mj_setPtrModel(mjModel* m) {
	char* ptr = (char*)m->buffer;
	int sz;

	// prepare sybmols needed by xmacro
	MJMODEL_POINTERS_PREAMBLE(m);

	// assign pointers with padding
#define X(type, name, nr, nc)                             \
	m->name = (type*)(ptr + SKIP((intptr_t)ptr));           \
	ASAN_POISON_MEMORY_REGION(ptr, PTRDIFF(m->name, ptr));  \
	ptr += SKIP((intptr_t)ptr) + sizeof(type)*(m->nr)*(nc);

	MJMODEL_POINTERS
#undef X

	// check size
	sz = (int)(ptr - (char*)m->buffer);
	if (m->nbuffer != sz) {
		printf("expected size: %d,  actual size: %d\n", m->nbuffer, sz);
		mju_error("mjModel buffer size mismatch");
	}
}


// allocate and initialize mjModel structure
mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int njnt,
					  int ngeom, int nsite, int ncam, int nlight,
					  int nmesh, int nmeshvert, int nmeshtexvert, int nmeshface, int nmeshgraph,
					  int nskin, int nskinvert, int nskintexvert, int nskinface,
					  int nskinbone, int nskinbonevert, int nhfield, int nhfielddata,
					  int ntex, int ntexdata, int nmat, int npair, int nexclude,
					  int neq, int ntendon, int nwrap, int nsensor,
					  int nnumeric, int nnumericdata, int ntext, int ntextdata,
					  int ntuple, int ntupledata, int nkey, int nmocap,
					  int nuser_body, int nuser_jnt, int nuser_geom, int nuser_site, int nuser_cam,
					  int nuser_tendon, int nuser_actuator, int nuser_sensor, int nnames) {
	intptr_t offset = 0;

	// allocate mjModel
	mjModel* m = (mjModel*)mju_malloc(sizeof(mjModel));
	if (!m) {
		mju_error("Could not allocate mjModel");
	}
	memset(m, 0, sizeof(mjModel));

	// set size parameters
	m->nq = nq;
	m->nv = nv;
	m->nu = nu;
	m->na = na;
	m->nbody = nbody;
	m->njnt = njnt;
	m->ngeom = ngeom;
	m->nsite = nsite;
	m->ncam = ncam;
	m->nlight = nlight;
	m->nmesh = nmesh;
	m->nmeshvert = nmeshvert;
	m->nmeshtexvert = nmeshtexvert;
	m->nmeshface = nmeshface;
	m->nmeshgraph = nmeshgraph;
	m->nskin = nskin;
	m->nskinvert = nskinvert;
	m->nskintexvert = nskintexvert;
	m->nskinface = nskinface;
	m->nskinbone = nskinbone;
	m->nskinbonevert = nskinbonevert;
	m->nhfield = nhfield;
	m->nhfielddata = nhfielddata;
	m->ntex = ntex;
	m->ntexdata = ntexdata;
	m->nmat = nmat;
	m->npair = npair;
	m->nexclude = nexclude;
	m->neq = neq;
	m->ntendon = ntendon;
	m->nwrap = nwrap;
	m->nsensor = nsensor;
	m->nnumeric = nnumeric;
	m->nnumericdata = nnumericdata;
	m->ntext = ntext;
	m->ntextdata = ntextdata;
	m->ntuple = ntuple;
	m->ntupledata = ntupledata;
	m->nkey = nkey;
	m->nmocap = nmocap;
	m->nuser_body = nuser_body;
	m->nuser_jnt = nuser_jnt;
	m->nuser_geom = nuser_geom;
	m->nuser_site = nuser_site;
	m->nuser_cam = nuser_cam;
	m->nuser_tendon = nuser_tendon;
	m->nuser_actuator = nuser_actuator;
	m->nuser_sensor = nuser_sensor;
	m->nnames = nnames;

#define X(name)                                    \
  if ((m->name) < 0) {                             \
	mju_warning("Invalid model: negative " #name); \
	mj_deleteModel(m);                             \
	return 0;                                      \
  }
	MJMODEL_INTS;
#undef X

  // compute buffer size
	m->nbuffer = 0;
#define X(type, name, nr, nc)                              \
  m->nbuffer += SKIP(offset) + sizeof(type)*(m->nr)*(nc);  \
  offset += SKIP(offset) + sizeof(type)*(m->nr)*(nc);

	MJMODEL_POINTERS
	#undef X

	  // allocate buffer
		m->buffer = mju_malloc(m->nbuffer);
	if (!m->buffer) {
		mju_error("Could not allocate mjModel buffer");
	}

	// clear, set pointers in buffer
	memset(m->buffer, 0, m->nbuffer);
#ifdef MEMORY_SANITIZER
  // Tell msan to treat the entire buffer as uninitialized
	__msan_allocated_memory(m->buffer, m->nbuffer);
#endif
	mj_setPtrModel(m);

	// set default options
	mj_defaultOption(&m->opt);
	mj_defaultVisual(&m->vis);
	mj_defaultStatistic(&m->stat);

	return m;
}



// 自定义坐标系到MuJoCo坐标系的变换函数
void transformToMuJoCo(vec3& customPos, vec3& customRot, vec3& mujocoPos, vec4& mujocoRot) {
	// 将坐标位置转换
	mujocoPos.x = customPos.x;
	mujocoPos.y = -customPos.z;
	mujocoPos.z = customPos.y;

	// 将旋转转换
	float halfRoll = customRot.x * 0.5f;
	float halfPitch = customRot.y * 0.5f;
	float halfYaw = customRot.z * 0.5f;
	float cosRoll = cos(halfRoll);
	float sinRoll = sin(halfRoll);
	float cosPitch = cos(halfPitch);
	float sinPitch = sin(halfPitch);
	float cosYaw = cos(halfYaw);
	float sinYaw = sin(halfYaw);

	mujocoRot.w = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
	mujocoRot.x = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
	mujocoRot.y = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
	mujocoRot.z = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
}

static inline vec3 convertToMujocoCoordinate(const vec3& srcCoord) {
	return { srcCoord.y, -srcCoord.x, -srcCoord.z };
	return { srcCoord.x, -srcCoord.z, srcCoord.y };
	return { srcCoord.y, -srcCoord.z, srcCoord.x };
}

void convertToMujocoCoordinate(const vec3& srcCoord, const vec3& srcRotation, vec3& mujocoCoord, vec4& mujocoQuaternion) {
	// 处理坐标转换
	mujocoCoord.x = srcCoord.y;
	mujocoCoord.y = -srcCoord.z;
	mujocoCoord.z = srcCoord.x;

	mujocoCoord = srcCoord;

	auto q = f_graph_convertRotation(srcRotation);
	// 处理旋转转换
	//mujocoQuaternion.w = q.w;
	//mujocoQuaternion.x = q.y;
	//mujocoQuaternion.y = -q.z;
	//mujocoQuaternion.z = q.x;
}

vec3 on_转换到MJ坐标系(const vec3& vert) {
	//f_mat44_构建旋转缩放矩阵;
	//static float32 a = 0;
	//auto mat = f_mat44_构建Y旋转矩阵(0);
	//mat *= f_mat44_构建X旋转矩阵(90);
	//mat *= f_mat44_构建Z旋转矩阵(45);
	vec3 v = vert;
	//v.x = vert.y;
	//v.y = vert.z;
	//v.z = -vert.x;
	//return v;
	//return { vert.x, vert.y, vert.z };
	//return { vert.x, vert.y, vert.z };
	return { vert.x, vert.z, -vert.y };
	//return mat * vert;
	//return vert;
	//return v;


	double radian = 90 * M_PI / 180.0;
	double cosTheta = cos(radian);
	double sinTheta = sin(radian);

	vec3 rotatedPoint;
	rotatedPoint.x = cosTheta * v.x - sinTheta * v.y;
	rotatedPoint.y = sinTheta * v.x + cosTheta * v.y;
	rotatedPoint.z = v.z;

	return rotatedPoint;
}


// 函数：将给定的旋转矩阵和位置向量进行坐标变换
Mat44f transformCoordinates(float32 rotationMatrix[3][3], float32 position[3]) {
	// 将旋转矩阵和位置向量变换到目标坐标系
	// 假设获取的旋转矩阵是rotationMatrix，位置向量是position
	Mat44f transformation = f_mat44_identity();


	//transformation.m00 = rotationMatrix[1][1];
	//transformation.m01 = rotationMatrix[0][1];
	//transformation.m02 = -rotationMatrix[2][1];
	//
	//transformation.m10 = rotationMatrix[1][0];
	//transformation.m11 = rotationMatrix[0][0];
	//transformation.m12 = -rotationMatrix[2][0];
	//
	//transformation.m20 = -rotationMatrix[1][2];
	//transformation.m21 = -rotationMatrix[0][2];
	//transformation.m22 = rotationMatrix[2][2];
	//
	//transformation.m30 = position[0];
	//transformation.m31 = position[1];
	//transformation.m32 = position[2];
	//
	//return transformation;
	

	// 构建变换矩阵
	transformation.m00 = rotationMatrix[0][0];
	transformation.m10 = rotationMatrix[0][1];
	transformation.m20 = rotationMatrix[0][2];
	transformation.m30 = position[0];

	transformation.m01 = rotationMatrix[1][0];
	transformation.m11 = rotationMatrix[1][1];
	transformation.m21 = rotationMatrix[1][2];
	transformation.m31 = position[1];

	transformation.m02 = rotationMatrix[2][0];
	transformation.m12 = rotationMatrix[2][1];
	transformation.m22 = rotationMatrix[2][2];
	transformation.m32 = position[2];

	transformation.m03 = 0;
	transformation.m13 = 0;
	transformation.m23 = 0;
	transformation.m33 = 1;

	return transformation;
}





S_PhysicsMujoco::S_PhysicsMujoco(std::wstring name) {
	m_模型 = nullptr;
	m_模拟数据 = nullptr;

	//mjModel* model = mj_makeEmpty();
	m_Name = g_名称生成器.f_Gen(name);
}

S_PhysicsMujoco::~S_PhysicsMujoco() {
	if (m_模拟数据) {
		mj_deleteData(m_模拟数据);
	}
	if (m_模型) {
		mj_deleteModel(m_模型);
	}

	g_名称生成器.f_Move(m_Name);
}

void S_PhysicsMujoco::f_Update() {
	mj_step(m_模型, m_模拟数据);
}

void S_PhysicsMujoco::f_重置变换(bool 解算) {
	if(!m_模型 || !m_模拟数据) return;

	mj_resetData(m_模型, m_模拟数据);

	if (解算) {
		mj_forward(m_模型, m_模拟数据);
		f_PhyMujoco_更新物体变换(this);
	}
	else {
		for (auto& e : m_Body) {
			((S_物体*)(e->m_绑定物体))->m_isUpdate = true;
		}
	}
	
	return;
}

//void S_PhysicsMujoco::f_构建场景XML() {
//	m_Body.clear();
//	m_XML.clear();
//	m_XML_asset.clear();
//
//
//	//std::string asset;
//	for (auto& 物理体 : m_预备构建物理体) {
//		
//		std::string 质量 = "0";
//		//物理体->m_变换.location;
//		//vec3 mujocoRot = {0,0,0};
//
//
//		auto& 几何体 = 物理体->m_几何体;
//
//		auto name = f_WStringToString(几何体->m_Name);
//		std::string 几何 = "<geom name = \"" + name + "\" ";
//		switch (几何体->m_Type) {
//			case E_物理几何体类型::E_碰撞几何_方体: {
//
//				S_物理几何_方体* box = (S_物理几何_方体*)(几何体->m_Data);
//				vec3 size = vec_Max(box->大小, { 0.00000001f, 0.00000001f, 0.00000001f });
//
//				几何 += "type = \"box\" ";
//				几何 += "size = \"" + f_浮点数值转字符串(size.x) + " " + f_浮点数值转字符串(size.y) + " " + f_浮点数值转字符串(size.z) + "\" ";
//				break;
//			}
//
//			case E_物理几何体类型::E_碰撞几何_平面: {
//
//				switch (物理体->m_Type) {
//					case E_物理体类型::E_物理体_刚体: continue;
//				}
//				//<quat x="-0.7071" y="0" z="0" w="0.7071"/> 
//
//				S_物理几何_平面* box = (S_物理几何_平面*)(几何体->m_Data);
//				vec3 p;
//				p.x = box->方向.x;
//				p.y = box->方向.y;
//				p.z = box->方向.z;
//				vec4 q = f_graph_quat两向量({ 0, 0, 1 }, p);
//
//				几何 += "type = \"plane\" ";
//				几何 += "size = \"10000000 0.01 10000000\" ";
//				几何 += "quat = \" " + f_浮点数值转字符串(q.w) + " " + f_浮点数值转字符串(q.x) + " " + f_浮点数值转字符串(q.y) + " " + f_浮点数值转字符串(q.z) + "\" "; ;
//				break;
//			}
//
//			case E_物理几何体类型::E_碰撞几何_网格: {
//				if (!几何体->m_绑定物体) continue;
//				auto file = f_ob_Save((S_物体*)几何体->m_绑定物体, S_引擎配置::g_工程文件路径 + "/" + name, L"stl", false, on_转换到MJ坐标系); //
//
//				m_XML_asset += "<mesh name = \"" + name + "\" file=\"" + file + "\"/>\n";
//
//				S_物理几何_网格* me = (S_物理几何_网格*)(几何体->m_Data);
//				几何 += "type = \"mesh\" ";
//				几何 += "mesh = \"" + name + "\" ";
//				几何 += "margin = \" 0.01 \" ";
//
//				//mujocoRot = me->方向;
//				break;
//			}
//
//			default:
//				continue;
//		}
//		几何 += "/>\n";
//
//
//		switch (物理体->m_Type) {
//			case E_物理体类型::E_物理体_空: {
//				break;
//			}
//			case E_物理体类型::E_物理体_刚体: {
//				//质量 = f_浮点数值转字符串(物理体->m_质量);
//
//				vec3 mujocoPos = 物理体->m_变换.location;
//				vec3 mujocoRot = 物理体->m_变换.rotation;
//
//				//transformToMuJoCo(物理体->m_变换.location, 物理体->m_变换.rotation, mujocoPos, mujocoRot);
//				//convertToMujocoCoordinate(物理体->m_变换.location, 物理体->m_变换.rotation, mujocoPos, mujocoRot);
//
//				//mujocoRot = f_graph_convertRotationToQuat({ 0,0,0 });
//				//mujocoRot = f_graph_convertRotation({ M_角度转弧度(90),M_角度转弧度(0), M_角度转弧度(-45) });
//				mujocoRot.x += 90;
//				vec4 Rot = f_graph_convertRotation(vec3{ float32(M_角度转弧度(mujocoRot.x)), float32(M_角度转弧度(mujocoRot.y)), float32(M_角度转弧度(mujocoRot.z)) });
//
//				m_XML += "<body name = \"" + f_WStringToString(物理体->m_Name) + "\" ";
//				m_XML += "pos = \"" + f_浮点数值转字符串(mujocoPos.x) + " " + f_浮点数值转字符串(mujocoPos.y) + " " + f_浮点数值转字符串(mujocoPos.z) + "\" ";
//				//m_XML += "quat = \"" + f_浮点数值转字符串(Rot.w) + " " + f_浮点数值转字符串(Rot.x) + " " + f_浮点数值转字符串(Rot.y) + " " + f_浮点数值转字符串(Rot.z) + "\" ";
//				m_XML += ">\n";
//
//				m_XML += 几何;
//				//m_XML += "<inertial pos = \"0 0 0\" mass = \"" + 质量 + "\" diaginertia = \"0.01 0.01 0.01\"/>\n";
//				m_XML += "<freejoint/>\n";
//
//				m_XML += "</body>\n";
//
//				//解算器->m_XML += "mass = \"" + 质量 + "\" ";
//
//				break;
//			}
//			case E_物理体类型::E_物理体_柔体: {
//				break;
//			}
//			case E_物理体类型::E_物理体_布料: {
//				break;
//			}
//			case E_物理体类型::E_物理体_控制: {
//				break;
//			}
//			case E_物理体类型::E_物理体_静态: {
//				m_XML += 几何;
//				//解算器->m_XML += "mass = \"0\" ";
//				break;
//			}
//		}
//
//		m_Body.push_back(物理体);
//	}
//
//	m_预备构建物理体.clear();
//}










S_物理体* f_PhyMujoco_场景物理体() {

	return nullptr;
}

S_Physics* f_PhysMujoco_创建物理引擎() {
	S_Physics* phy = new S_PhysicsMujoco;
	return phy;
}

void f_PhysMujoco_重构(S_PhysicsMujoco* 解算器, const std::string& path) {
	
	//解算器->f_构建场景XML();

	if (解算器->m_XML.empty()) return;

	if (解算器->m_模型) {
		mj_deleteData(解算器->m_模拟数据);
		mj_deleteModel(解算器->m_模型);
		
		解算器->m_模拟数据 = nullptr;
		解算器->m_模型 = nullptr;
	}

	std::string 文件头 = "<?xml version = \"1.0\" encoding = \"utf-8\"?>\n";
	文件头 += "<mujoco model = \"" + f_WStringToString(解算器->m_Name) + "\">\n";
	文件头 += "<option timestep = \"0.01\" solver = \"Newton\" integrator = \"RK4\" gravity = \"0 -9.81 0\" />\n";
	文件头 += "<compiler angle=\"radian\" coordinate=\"local\"/>\n <default/>\n";
	//文件头 += "<compiler angle=\"radian\" coordinate=\"local\" eulerseq=\"zyx\" />\n <default/>\n";

	

	std::string XML = 文件头;

	XML += "<asset>\n";
	XML += 解算器->m_XML_asset;
	XML += "</asset>\n";

	XML += "<worldbody>\n " + 解算器->m_XML;
	XML += "</worldbody>\n";
	XML += "</mujoco>\n";
	

	FILE* f = fopen(path.c_str(), "wt");
	if (f) {
		fwrite(XML.c_str(), sizeof(char), XML.length(), f);
		fclose(f);
	}


	char error[1000];
	解算器->m_模型 = mj_loadXML(path.c_str(), nullptr, error, 1000);
	解算器->m_XML.clear();
	解算器->m_XML_asset.clear();


	if (解算器->m_模型) {
		解算器->m_模拟数据 = mj_makeData(解算器->m_模型);
	}
	else {
		解算器->m_模拟数据 = nullptr;
		return;
	}
	
	

	//解算器->m_模型->geom
	for (auto& e : 解算器->m_Body) {
		//auto name = f_WStringToString(e->m_几何体->m_Name);
		int32 object_id = -1;

		switch (e->m_物理体类型) {
			case E_物理体类型::E_物理体_静态: {
				auto name = f_WStringToString(e->m_几何体->m_Name);
				object_id = mj_name2id(解算器->m_模型, mjOBJ_GEOM, name.c_str());
				break;
			}
			case E_物理体类型::E_物理体_刚体: {
				auto name = f_WStringToString(e->m_Name);
				//object_id = mj_name2id(解算器->m_模型, mjOBJ_GEOM, name.c_str());
				object_id = mj_name2id(解算器->m_模型, mjOBJ_BODY, name.c_str());
				break;
			}
		}

		e->m_ID = object_id;
	}

	//mjvGeom geom;
	//mjvScene scene;
	//mjvOption opt;
	//mjvPerturb perturb;
	//mjv_defaultOption(&opt);
	//mjv_defaultPerturb(&perturb);
	//mjv_makeScene(解算器->m_模型, &scene, 1);
	//mjv_addGeoms(解算器->m_模型, 解算器->m_模拟数据, &opt, &perturb, 0xFFFFFFFF, &scene);
	//mju_copy3()
	return;


	if (解算器->m_模拟数据) {
		uint32 body_num = 解算器->m_Body.size();

		for (uint32 i = 0; i < 解算器->m_模型->nbody; ++i) {
			for (uint32 j = 0; j < 解算器->m_模型->body_geomnum[i]; ++j) {
				int32 几何体位置偏移 = 解算器->m_模型->body_geomadr[i];

				S_物理体* body;
				if (body_num <= 几何体位置偏移) {
					//解算器->m_Body.push_back(body);
				}
				else {
					//body = 解算器->m_Body[几何体位置偏移];
				}

				S_物理几何体* geom = (S_物理几何体*)body->m_几何体;
				if (!body->m_几何体) {
					geom = new S_物理几何体(E_碰撞几何_网格);
					body->m_几何体 = geom;
				}

				std::string name = &(解算器->m_模型->names[解算器->m_模型->name_bodyadr[i]]);
				body->m_Name = f_StringToWString(name);


				body->m_物理引擎枚举	= E_物理引擎::E_物理引擎_Mujoco;
				body->m_ID				= 几何体位置偏移;
				//body->m_Type			= E_物理体类型::E_物理体_刚体;
				
				//switch (mj->m_模型->geom_type[几何体位置偏移]) {
				//	case mjtGeom::mjGEOM_PLANE:
				//		geom->m_Type = E_物理几何体类型::E_碰撞几何_平面;
				//		break;
				//	case mjtGeom::mjGEOM_SPHERE:
				//		geom->m_Type = E_物理几何体类型::E_碰撞几何_球体;
				//		break;
				//	case mjtGeom::mjGEOM_CAPSULE:	break;
				//	case mjtGeom::mjGEOM_ELLIPSOID:	break;
				//	case mjtGeom::mjGEOM_CYLINDER:	break;
				//	case mjtGeom::mjGEOM_BOX:
				//		geom->m_Type = E_物理几何体类型::E_碰撞几何_方体;
				//		break;
				//	case mjtGeom::mjGEOM_MESH: {
				//		geom->m_Type = E_物理几何体类型::E_碰撞几何_网格;
				//		break;
				//	}
				//}
			}
		}

		
		//for (uint32 i = 解算器->m_模型->ngeom; i < body_num; ++i) {
			//delete 解算器->m_Body.back();
			//解算器->m_Body.pop_back();
		//}
	}

	return;

	/*auto body_ptr = mj->m_Body.data();
	phy->m_XML = "<mujoco> \n";
	{
		{
			phy->m_XML += "<compiler coordinate = \"local\"  eulerseq = \"xyz\" /> \n";
		}
		phy->m_XML += "	<worldbody> \n";
		for (uint32 i = 0; i < body_num; ++i) {
			auto& body = body_ptr[i];
			phy->m_XML += body->m_XML;
		}
		phy->m_XML += "	</worldbody> \n";
	}
	phy->m_XML += "</mujoco> \n";*/
	//for (uint32 i = 0; i < body_num; ++i) {
	//	body_ptr[i]->m_ID = i;
	//	int32 id = mj_name2id(mj->m_模型, mjOBJ_BODY, "");
	//}
	//
}

void f_PhysMujoco_销毁物理引擎(S_Physics* phy) {
	delete phy;
}

void f_PhyMujoco_解算(S_PhysicsMujoco* mj, float32 时间步进) {
	if (!mj->m_模拟数据) return;

	//mj->m_步进ID;
	//int bodyid = mj_name2id(mj->m_模型, mjOBJ_BODY, "myfloatingbody");
	//mj->m_模拟数据->time = 1 / 60;
	//mj->m_模型->opt.gravity[0] = 0;
	//mj->m_模型->opt.gravity[1] = -9.81;
	//mj->m_模型->opt.gravity[2] = 0;
	mj->m_模型->opt.timestep = 时间步进;
	mj->m_模型->opt.o_margin = 0.02;
	mj->m_模型->opt.noslip_iterations = 6;
	
	mj_step(mj->m_模型, mj->m_模拟数据);

	f_PhyMujoco_更新物体变换(mj);
	return;

	//mat3X3 mat = *((mj->m_模拟数据->geom_xmat);
	uint32 body_num = mj->m_Body.size();
	auto body_ptr = mj->m_Body.data();

	//for (uint32 i = 0; i < mj->m_模型->ngeom; ++i) {
	//	switch (mj->m_模型->geom_type[i]) {
	//		case mjtGeom::mjGEOM_PLANE:break;
	//		case mjtGeom::mjGEOM_SPHERE:break;
	//		case mjtGeom::mjGEOM_CAPSULE:break;
	//		case mjtGeom::mjGEOM_ELLIPSOID:break;
	//		case mjtGeom::mjGEOM_CYLINDER:break;
	//		case mjtGeom::mjGEOM_BOX:break;
	//		case mjtGeom::mjGEOM_MESH: {
	//			break;
	//		}
	//	}
	//}

	/*for (uint32 i = 0; i < mj->m_模型->nbody; ++i) {
		int32 几何体ID = mj->m_模型->body_geomadr[i];
		if (几何体ID >= 0) {
			if (!(body_ptr[几何体ID]->m_绑定物体)) continue;

			mat3X3 rot;
			vec3 pos;
			for (uint8 i = 0; i < 9; ++i) {
				((float32*)&rot)[i] = mj->m_模拟数据->geom_xmat[i + 几何体ID * 9];
			}
			for (uint8 i = 0; i < 3; ++i) {
				((float32*)&pos)[i] = mj->m_模拟数据->geom_xpos[i + 几何体ID * 3];
			}
			f_mat4_构建缩放矩阵(body_ptr[几何体ID]->m_变换.scale);

			S_物体* ob = (S_物体*)body_ptr[几何体ID]->m_绑定物体;
			//Mat44f mat = f_mat44_构建(rot, pos) * f_mat4_构建缩放矩阵(body_ptr[ob_id]->m_变换.scale);
			Mat44f mat = f_mat44_构建(rot, pos);
			f_ob_set矩阵(ob, mat);
		}
	}*/

	//mj->m_模型->sensor_objid;

	for (uint32 ob_id = 0; ob_id < body_num; ++ob_id) {
		uint32 几何体ID = body_ptr[ob_id]->m_ID;

		if (几何体ID == -1 || !(body_ptr[ob_id]->m_绑定物体)) continue;
		//int32 id = mj_name2id(mj->m_模型, mjOBJ_BODY, body_ptr[几何体ID]->m_Name.c_str());
		//几何体ID = id;
		--几何体ID;

		mat3X3 rot;
		vec3 pos;
		for (uint8 i = 0; i < 9; ++i) {
			((float32*)&rot)[i] = mj->m_模拟数据->geom_xmat[i + 几何体ID * 9];
		}
		for (uint8 i = 0; i < 3; ++i) {
			((float32*)&pos)[i] = mj->m_模拟数据->geom_xpos[i + 几何体ID * 3];
		}
		f_mat4_构建缩放矩阵(body_ptr[ob_id]->m_变换.scale);

		S_物体* ob = (S_物体*)body_ptr[ob_id]->m_绑定物体;
		//rot = f_mat_3x3转置(rot);
		//Mat44f mat = f_mat44_构建(rot, pos);// *f_mat4_构建缩放矩阵(body_ptr[ob_id]->m_变换.scale);
		//mat = f_mat44_构建X旋转矩阵(-90) * mat;
		Mat44f mat = f_mat33_to_44(rot);
		mat = f_mat44_构建X旋转矩阵(-90) * mat;
		mat = f_mat44_构建(mat, pos);
		f_ob_set矩阵((S_物体*)body_ptr[ob_id]->m_绑定物体, mat);
		continue;
		//body_ptr[几何体ID]->m_Matrix = f_mat4_构建移动矩阵(pos) * f_mat44_构建X旋转矩阵(-90) * f_mat44_构建Z旋转矩阵(0) * f_mat33_to_44(rot);
		//body_ptr[几何体ID]->m_Matrix = f_mat4_构建移动矩阵(pos);
		body_ptr[ob_id]->m_Matrix = f_mat44_构建X旋转矩阵(-90) * (f_mat4_构建缩放矩阵({1,-1,1}) * mat);
		f_ob_set矩阵((S_物体*)body_ptr[ob_id]->m_绑定物体, body_ptr[ob_id]->m_Matrix);
	}
}

void f_PhyMujoco_添加物理体(S_PhysicsMujoco* 解算器, std::vector<S_物理体*>& body) {
	解算器->m_Body = body;


}

Inline vec4 f_vectorToQuat(float32 x, float32 y, float32 z) {
	vec4 q;

	double crossX = y * 0.0 - z * 1.0;
	double crossY = z * 1.0 - x * 0.0;
	double crossZ = x * 0.0 - y * 0.0;

	// Calculate the magnitude of the cross product
	double norm = sqrt(crossX * crossX + crossY * crossY + crossZ * crossZ);

	// Normalize the cross product
	if (norm > 0.0) {
		crossX /= norm;
		crossY /= norm;
		crossZ /= norm;
	}

	// Calculate the angle between the given vector and the up vector
	double dot = y * 1.0;
	double angle = acos(dot / (sqrt(x * x + y * y + z * z) * 1.0));

	// Calculate the quaternion components
	double halfAngle = angle / 2.0;
	double sinHalfAngle = sin(halfAngle);
	q.w = cos(halfAngle);
	q.x = crossX * sinHalfAngle;
	q.y = crossY * sinHalfAngle;
	q.z = crossZ * sinHalfAngle;

	return q;
}

static std::string f_PhyMujoco_构建几何体XML(S_PhysicsMujoco* 解算器, S_物理几何体* 几何体) {
	auto name = f_WStringToString(几何体->m_Name);
	std::string 几何 = "<geom name = \"" + name + "\" ";
	
	switch (几何体->m_Type) {
		case E_物理几何体类型::E_碰撞几何_方体: {

			S_物理几何_方体* box = (S_物理几何_方体*)(几何体->m_Data);
			vec3 size = vec_Max(box->大小, { 0.00000001f, 0.00000001f, 0.00000001f }) * 0.5;

			几何 += "type = \"box\" ";
			几何 += "size = \"" + f_浮点数值转字符串(size.x) + " " + f_浮点数值转字符串(size.y) + " " + f_浮点数值转字符串(size.z) + "\" ";

			break;
		}

		case E_物理几何体类型::E_碰撞几何_圆柱: {

			S_物理几何_圆柱* box = (S_物理几何_圆柱*)(几何体->m_Data);
			vec3 size = vec_Max(box->大小, { 0.00000001f, 0.00000001f, 0.00000001f }) * 0.5;
			vec4 q = f_graph_quat两向量({ 0, 0, 1 }, { 0, 1, 0 });

			几何 += "type = \"cylinder\" ";
			几何 += "size = \"" + f_浮点数值转字符串(size.x) + " " + f_浮点数值转字符串(size.y) + "\" ";
			//几何 += "quat = \" " + f_浮点数值转字符串(q.w) + " " + f_浮点数值转字符串(q.x) + " " + f_浮点数值转字符串(q.y) + " " + f_浮点数值转字符串(q.z) + "\" ";
			
			break;
		}

		case E_物理几何体类型::E_碰撞几何_胶囊: {

			S_物理几何_胶囊* box = (S_物理几何_胶囊*)(几何体->m_Data);
			vec2 size = vec_Max(box->大小, { 0.00000001f, 0.00000001f }) * 0.5;

			几何 += "type = \"capsule\" ";
			几何 += "size = \"" + f_浮点数值转字符串(size.x) + " " + f_浮点数值转字符串(size.y) + "\" ";

			break;
		}

		case E_物理几何体类型::E_碰撞几何_球体: {

			S_物理几何_球体* box = (S_物理几何_球体*)(几何体->m_Data);
			float size = DEF_Max(box->大小, 0.00000001f) * 0.5;

			几何 += "type = \"sphere\" ";
			几何 += "size = \"" + f_浮点数值转字符串(size) + "\" ";

			break;
		}

		case E_物理几何体类型::E_碰撞几何_平面: {
			S_物理几何_平面* box = (S_物理几何_平面*)(几何体->m_Data);
			vec3 p;
			p.x = box->方向.x;
			p.y = box->方向.y;
			p.z = box->方向.z;
			vec4 q = f_graph_quat两向量({ 0, 0, 1 }, p);

			几何 += "type = \"plane\" ";
			几何 += "size = \"10000000 0.01 10000000\" ";
			几何 += "quat = \" " + f_浮点数值转字符串(q.w) + " " + f_浮点数值转字符串(q.x) + " " + f_浮点数值转字符串(q.y) + " " + f_浮点数值转字符串(q.z) + "\" ";

			p = convertToMujocoCoordinate(box->位置);
			p = box->位置;
			几何 += "pos = \" " + f_浮点数值转字符串(p.x) + " " + f_浮点数值转字符串(p.y) + " " + f_浮点数值转字符串(p.z) + "\" ";

			break;
		}

		case E_物理几何体类型::E_碰撞几何_网格: {
			if (!几何体->m_绑定网格物体) return "";

			auto file = f_ob_Save((S_物体*)几何体->m_绑定网格物体, S_引擎配置::g_工程文件路径 + S_引擎配置::g_资源文件夹 + name, L"stl", false, on_转换到MJ坐标系); //

			解算器->m_XML_asset += "<mesh name = \"mesh_" + name + "\" file=\"" + file + "\"/>\n";

			S_物理几何_网格* me = (S_物理几何_网格*)(几何体->m_Data);
			几何 += "type = \"mesh\" ";
			几何 += "mesh = \"mesh_" + name + "\" ";
			//几何 += "size = \" 1 1 1 \" ";
			//几何 += "margin = \" 0.01 \" ";
			//vec4 q = f_graph_quat两向量({ 0, 0, 1 }, { 0, 1, 0 });
			//几何 += "quat = \" " + f_浮点数值转字符串(q.w) + " " + f_浮点数值转字符串(q.x) + " " + f_浮点数值转字符串(q.y) + " " + f_浮点数值转字符串(q.z) + "\" ";
			//mujocoRot = me->方向;
			break;
		}

		default:
			return "";
	}

	几何 += "mass=\"" + f_浮点数值转字符串(几何体->g_材质.m_重量) + "\" ";
	几何 += "density=\"" + f_浮点数值转字符串(几何体->g_材质.m_密度) + "\" ";
	//几何 += "elasticity=\"" + f_浮点数值转字符串(几何体->m_材质.m_弹性) + "\" ";
	//几何 += "damping=\"" + f_浮点数值转字符串(几何体->m_材质.m_阻尼) + "\" ";
	几何 += "friction=\"" + f_浮点数值转字符串(几何体->g_材质.m_摩擦.x) + " " + f_浮点数值转字符串(几何体->g_材质.m_摩擦.y) + " " + f_浮点数值转字符串(几何体->g_材质.m_摩擦.z) + "\" ";
	//几何 += "inertia=\"" + f_浮点数值转字符串(几何体->m_材质.m_旋转惯性.x) + " " + f_浮点数值转字符串(几何体->m_材质.m_旋转惯性.y) + " " + f_浮点数值转字符串(几何体->m_材质.m_旋转惯性.z) + "\" ";

	几何 += "margin=\"" + f_浮点数值转字符串(几何体->g_材质.m_边缘距离) + "\" ";
	几何 += "solref=\"" + f_浮点数值转字符串(几何体->g_材质.m_接触参考.x) + " " + f_浮点数值转字符串(几何体->g_材质.m_接触参考.y) + "\" ";
	几何 += "solimp=\"" + f_浮点数值转字符串(几何体->g_材质.m_接触实施.x) + " " + f_浮点数值转字符串(几何体->g_材质.m_接触实施.y) + "\" ";
	几何 += "solmix=\"" + f_浮点数值转字符串(几何体->g_材质.m_接触混合) + "\" ";

	几何 += "contype=\"" + f_整数值转字符串(几何体->g_材质.m_碰撞位掩码) + "\" ";
	几何 += "conaffinity=\"" + f_整数值转字符串(几何体->g_材质.m_接触的频率) + "\" ";
	几何 += "condim=\"" + f_整数值转字符串(几何体->g_材质.m_接触自由度) + "\" ";

	几何 += "/>\n";

	return 几何;
}

static std::string f_PhyMujoco_构建物理体XML(S_物理体* 物理体, const std::string 几何XML, const std::string name, const vec3& 父物体坐标, uint32 层) {
	std::string 物理体XML;

	std::string 链接类型;
	switch (物理体->m_链接类型) {
		case E_物理体链接类型::e_自由: { 链接类型 = "free"; break; }
		case E_物理体链接类型::e_铰链: { 链接类型 = "hinge"; break; }
		case E_物理体链接类型::e_滑动: { 链接类型 = "slide"; break; }
		case E_物理体链接类型::e_球型: { 链接类型 = "ball"; break; }
		case E_物理体链接类型::e_固定: { 链接类型 = "fixed"; break; }

		case E_物理体链接类型::e_旋转滑动:	{ 链接类型 = "slide2_joint"; break; }
		case E_物理体链接类型::e_圆柱活塞:		{ 链接类型 = "cylinder_joint"; break; }
		case E_物理体链接类型::e_约束球:	{ 链接类型 = "spherical_joint"; break; }
	}

	switch (物理体->m_物理体类型) {
		case E_物理体类型::E_物理体_空: {
			break;
		}
		case E_物理体类型::E_物理体_刚体: {
			//auto 质量 = f_浮点数值转字符串(物理体->m_质量);

			vec3 mujocoPos = 物理体->m_变换.location - 父物体坐标;
			
			//mujocoRot = {};
			vec4 Rot;// = f_graph_convertRotation(vec3{ float32(M_角度转弧度(mujocoRot.x)), float32(M_角度转弧度(mujocoRot.y)), float32(M_角度转弧度(mujocoRot.z)) });
			//mujocoRot = vec3{ float32(M_角度转弧度(mujocoRot.x)), float32(M_角度转弧度(mujocoRot.y)), float32(M_角度转弧度(mujocoRot.z)) };
			vec3 mujocoRot{};
			switch (物理体->m_几何体->m_Type) {
				//case E_物理几何体类型::E_碰撞几何_网格: {
				//	static float32 a = 0;
				//	Rot = f_graph_convertRotation(vec3{ float32(M_角度转弧度(0)), float32(M_角度转弧度(0)), float32(M_角度转弧度(0)) });
				//	a += 1;
				//	break;
				//}
				default: {
					//mujocoRot = {};
					mujocoRot = 物理体->m_变换.rotation;
					if(!层) mujocoRot.x += 90;
					else {
						vec3 tpos = mujocoPos;
						mujocoPos.y = tpos.z;
						mujocoPos.z = -tpos.y;
					}
					Rot = f_graph_convertRotation(vec3{ float32(M_角度转弧度(mujocoRot.x)), float32(M_角度转弧度(mujocoRot.y)), float32(M_角度转弧度(mujocoRot.z)) });


					break;
				}
			}
			//mujocoRot.x = M_角度转弧度(mujocoRot.x);
			//mujocoRot.y = M_角度转弧度(mujocoRot.y);
			//mujocoRot.z = M_角度转弧度(mujocoRot.z);


			//解算器->m_XML += "<body name = \"" + f_WStringToString(物理体->m_Name) + "\" ";
			物理体XML += "pos = \"" + f_浮点数值转字符串(mujocoPos.x) + " " + f_浮点数值转字符串(mujocoPos.y) + " " + f_浮点数值转字符串(mujocoPos.z) + "\" ";
			物理体XML += "quat = \"" + f_浮点数值转字符串(Rot.w) + " " + f_浮点数值转字符串(Rot.x) + " " + f_浮点数值转字符串(Rot.y) + " " + f_浮点数值转字符串(Rot.z) + "\" ";
			//物理体XML += "euler = \"" + f_浮点数值转字符串(mujocoRot.x) + " " + f_浮点数值转字符串(mujocoRot.y) + " " + f_浮点数值转字符串(mujocoRot.z) + "\" ";
			物理体XML += ">\n";

			//物理体XML += "<inertial pos = \"0 0 0\" mass = \"" + 质量 + "\" diaginertia = \"0.01 0.01 0.01\"/>\n";
			//物理体XML += "<inertial diaginertia = \"0.01 0.01 0.01\"/>\n";

			物理体XML += 几何XML;
			
			//if (物理体->m_质量 != 0) {
			//	物理体XML += "<inertial pos = \"0 0 0\" mass = \"" + 质量 + "\" diaginertia = \"0.01 0.01 0.01\"/>\n";
			//	//物理体XML += " diaginertia = \"0.01 0.01 0.01\"";
			//}
			//else {
			//	//物理体XML += "<mass = \" 0 \" />\n";
			//	物理体XML += "<mass = \"" + 质量 + "\" />\n";
			//}
			//物理体XML += "/>\n";

			//物理体XML += "<freejoint/>\n";
			switch (物理体->m_链接类型) {
				case E_物理体链接类型::e_自由:{
					物理体XML += "<joint type=\"" + 链接类型 + "\"" + " />\n";
					break;
				}
				case E_物理体链接类型::e_固定: {
					物理体XML += "<joint type=\"" + 链接类型 + "\"" + " />\n";
					break;
				}
				//case E_物理体链接类型::e_铰链:
				//case E_物理体链接类型::e_滑动:
				case E_物理体链接类型::e_球型:
					物理体->m_关节范围.x = DEF_Max(0, 物理体->m_关节范围.x);
					物理体->m_关节范围.y = DEF_Max(0, 物理体->m_关节范围.y);
				default:{
					auto 链接坐标 = ((S_物体*)(物理体->m_虚拟链接轴绑定物体))->m_变换.location;
					auto 轴 = f_浮点数值转字符串(物理体->m_链接轴向.x) + " " + f_浮点数值转字符串(物理体->m_链接轴向.y) + " " + f_浮点数值转字符串(物理体->m_链接轴向.z);
					auto 物理系数 = "damping = \"" + f_浮点数值转字符串(物理体->m_关节阻尼) + "\" stiffness=\"" + f_浮点数值转字符串(物理体->m_关节刚度) + "\" ";
					auto 物理限定 = "limited = \"" + f_布尔转字符串(物理体->m_限定范围) + "\" range=\"" + f_浮点数值转字符串(物理体->m_关节范围.x) + " " + f_浮点数值转字符串(物理体->m_关节范围.y) + "\" ";
					
					//auto 轴旋转 = "quat = \"" + f_浮点数值转字符串(链接坐标.x) + " " + f_浮点数值转字符串(链接坐标.x) + " " + f_浮点数值转字符串(链接坐标.z) + " " + f_浮点数值转字符串(-链接坐标.y) + "\" ";
					auto 轴旋转 = f_graph_convertRotation(vec3{ float32(90), float32(M_角度转弧度(0)), float32(M_角度转弧度(0)) });
					//链接坐标 = f_graph_quatTranformInv(轴旋转, 链接坐标);
					auto 轴偏移 = "pos = \"" + f_浮点数值转字符串(链接坐标.x) + " " + f_浮点数值转字符串(链接坐标.z) + " " + f_浮点数值转字符串(-链接坐标.y) + "\" ";

					物理体XML += "<joint name = \"" + name + "joint " + "\" type=\"" + 链接类型 + "\" axis=\"" + 轴 + "\" " + 轴偏移 + 物理系数 + 物理限定 + " />\n";
				}
			}
			

			//解算器->m_XML += "</body>\n";

			//物理体XML += "mass = \"" + 质量 + "\" ";

			break;
		}
		case E_物理体类型::E_物理体_柔体: {
			break;
		}
		case E_物理体类型::E_物理体_布料: {
			
			break;
		}
		case E_物理体类型::E_物理体_控制: {
			break;
		}
		case E_物理体类型::E_物理体_静态: {
			物理体XML += 几何XML;
			//解算器->m_XML += "mass = \"0\" ";
			break;
		}
	}
	return 物理体XML;
}

static std::string f_PhyMujoco_构建物理体(S_PhysicsMujoco* 解算器, S_物理体* body, const vec3& 父物体坐标, uint32 层) {
	if(!body->m_几何体) return "";

	std::string 物理体XML;

	body->mf_销毁 = nullptr;
	auto& 几何体 = body->m_几何体;
	
	std::string 几何 = f_PhyMujoco_构建几何体XML(解算器, 几何体);
	

	auto name = f_WStringToString(body->m_Name);

	switch (body->m_物理体类型) {
		case E_物理体类型::E_物理体_静态: {
			物理体XML += 几何;
			break;
		}
		//case E_物理体类型::E_物理体_布料: {
		//	物理体XML += 几何;
		//	break;
		//}
		default: {
			物理体XML += "<body name = \"" + name + "\" ";
			物理体XML += f_PhyMujoco_构建物理体XML(body, 几何, name, 父物体坐标, 层);

			for (auto& 子物体 : body->m_链接子物体) {
				物理体XML += f_PhyMujoco_构建物理体(解算器, 子物体, body->m_变换.location, 层 + 1);
			}


			物理体XML += "</body>\n";
			解算器->m_Body.push_back(body);
			break;
		}
	}

	return 物理体XML;
}

void f_PhyMujoco_构建物理体(S_PhysicsMujoco* 解算器, S_PhyBodyArray* body) {
	//for (uint32 i = 0; i < body->count; ++i) {
	//	auto& 物理体 = body->ptr_userData[i];
	//	物理体->m_构建更新 = false;
	//
	//	解算器->m_预备构建物理体.insert(物理体);
	//	if (物理体->m_链接父物体 && 物理体->m_链接父物体->m_几何体) {
	//		f_PhyMujoco_构建物理体(解算器, 物理体->m_链接父物体);
	//	}
	//}
	//return;

	if (解算器->m_XML.empty()) {
		解算器->m_Body.clear();
		解算器->m_XML_asset.clear();
	}


	//std::string asset;
	for (uint32 i = 0; i < body->count; ++i) {
		auto& 物理体 = body->ptr_userData[i];

		解算器->m_XML += f_PhyMujoco_构建物理体(解算器, 物理体, {}, 0);
		//解算器->m_Body.push_back(物理体);
		continue;

		std::string 质量 = "0";
		//物理体->m_变换.location;
		//vec3 mujocoRot = {0,0,0};
		auto& 几何体 = body->ptr_userData[i]->m_几何体;
		//auto name = f_WStringToString(几何体->m_Name);
		//std::string 几何 = "<geom name = \"" + name + "\" ";
		//switch (几何体->m_Type) {
		//	case E_物理几何体类型::E_碰撞几何_方体: {
		//		
		//		S_物理几何_方体* box = (S_物理几何_方体*)(几何体->m_Data);
		//		vec3 size = vec_Max(box->大小, {0.00000001f, 0.00000001f, 0.00000001f });
		//
		//		几何 += "type = \"box\" ";
		//		几何 += "size = \"" + f_浮点数值转字符串(size.x) + " " + f_浮点数值转字符串(size.y) + " " + f_浮点数值转字符串(size.z) + "\" ";
		//
		//		质量 = f_浮点数值转字符串(box->质量);
		//		break;
		//	}
		//
		//	case E_物理几何体类型::E_碰撞几何_平面: {
		//		
		//		switch (物理体->m_Type) {
		//			case E_物理体类型::E_物理体_刚体: continue;
		//		}
		//		//<quat x="-0.7071" y="0" z="0" w="0.7071"/> 
		//
		//		S_物理几何_平面* box = (S_物理几何_平面*)(几何体->m_Data);
		//		vec3 p ;
		//		p.x = box->方向.x;
		//		p.y = box->方向.y;
		//		p.z = box->方向.z;
		//		vec4 q = f_graph_quat两向量({ 0, 0, 1 }, p);
		//		
		//		几何 += "type = \"plane\" ";
		//		几何 += "size = \"10000000 0.01 10000000\" ";
		//		几何 += "quat = \" " + f_浮点数值转字符串(q.w) + " " + f_浮点数值转字符串(q.x) + " " + f_浮点数值转字符串(q.y) + " " + f_浮点数值转字符串(q.z) + "\" "; ;
		//		break;
		//	}
		//
		//	case E_物理几何体类型::E_碰撞几何_网格: {
		//		if(!几何体->m_绑定物体) continue;
		//		auto file = f_ob_Save((S_物体*)几何体->m_绑定物体, S_引擎配置::g_工程文件路径 + "/" + name, L"stl", false, on_转换到MJ坐标系); //
		//
		//		解算器->m_XML_asset += "<mesh name = \"" + name + "\" file=\"" + file + "\"/>\n";
		//
		//		S_物理几何_网格* me = (S_物理几何_网格*)(几何体->m_Data);
		//		几何 += "type = \"mesh\" ";
		//		几何 += "mesh = \"" + name + "\" ";
		//		几何 += "margin = \" 0.01 \" ";
		//
		//		//mujocoRot = me->方向;
		//		break;
		//	}
		//
		//	default:
		//		continue;
		//}
		//几何 += "/>\n";
		
		//std::string 几何 = f_PhyMujoco_构建几何体XML(解算器, 几何体);


		//std::string 物理体XML = "<body name = \"" + f_WStringToString(物理体->m_Name) + "\" ";
		//switch (物理体->m_Type) {
		//	case E_物理体类型::E_物理体_空: {
		//		break;
		//	}
		//	case E_物理体类型::E_物理体_刚体: {
		//		质量 = f_浮点数值转字符串(物理体->m_质量);
		//
		//		vec3 mujocoPos = 物理体->m_变换.location;
		//		vec3 mujocoRot = 物理体->m_变换.rotation;
		//		
		//		//transformToMuJoCo(物理体->m_变换.location, 物理体->m_变换.rotation, mujocoPos, mujocoRot);
		//		//convertToMujocoCoordinate(物理体->m_变换.location, 物理体->m_变换.rotation, mujocoPos, mujocoRot);
		//
		//		//mujocoRot = f_graph_convertRotationToQuat({ 0,0,0 });
		//		//mujocoRot = f_graph_convertRotation({ M_角度转弧度(90),M_角度转弧度(0), M_角度转弧度(-45) });
		//		mujocoRot.x += 90;
		//		vec4 Rot = f_graph_convertRotation(vec3{ float32(M_角度转弧度(mujocoRot.x)), float32(M_角度转弧度(mujocoRot.y)), float32(M_角度转弧度(mujocoRot.z)) });
		//
		//		//解算器->m_XML += "<body name = \"" + f_WStringToString(物理体->m_Name) + "\" ";
		//		解算器->m_XML += "pos = \"" + f_浮点数值转字符串(mujocoPos.x) + " " + f_浮点数值转字符串(mujocoPos.y) + " " + f_浮点数值转字符串(mujocoPos.z) + "\" ";
		//		解算器->m_XML += "quat = \"" + f_浮点数值转字符串(Rot.w) + " " + f_浮点数值转字符串(Rot.x) + " " + f_浮点数值转字符串(Rot.y) + " " + f_浮点数值转字符串(Rot.z) + "\" ";
		//		解算器->m_XML += ">\n";
		//
		//		解算器->m_XML += 几何;
		//		解算器->m_XML += "<inertial pos = \"0 0 0\" mass = \"" + 质量 + "\" diaginertia = \"0.01 0.01 0.01\"/>\n";
		//		解算器->m_XML += "<freejoint/>\n";
		//
		//		//解算器->m_XML += "</body>\n";
		//
		//		//解算器->m_XML += "mass = \"" + 质量 + "\" ";
		//
		//		break;
		//	}
		//	case E_物理体类型::E_物理体_柔体: {
		//		break;
		//	}
		//	case E_物理体类型::E_物理体_布料: {
		//		break;
		//	}
		//	case E_物理体类型::E_物理体_控制: {
		//		break;
		//	}
		//	case E_物理体类型::E_物理体_静态: {
		//		解算器->m_XML += 几何;
		//		//解算器->m_XML += "mass = \"0\" ";
		//		break;
		//	}
		//}
		//
		//for (auto& e : 物理体->m_链接子物体) {
		//	
		//}
		//
		//
		//物理体XML += "</body>\n";
		//解算器->m_XML += 物理体XML;
		//解算器->m_Body.push_back(物理体);
	}

}

void f_PhyMujoco_重构(S_PhysicsMujoco* 解算器) {
	
	char error[1000];
	解算器->m_模型 = mj_loadXML(解算器->m_XML.c_str(), NULL, error, 1000);
	
	解算器->m_XML.clear();
}

std::vector<S_物理体*> f_PhyMujoco_get几何图形(S_PhysicsMujoco* mj, E_物理几何体类型 几何类型) {
	uint32 num = mj->m_Body.size();
	auto boyd_ptr = mj->m_Body.data();

	std::vector<S_物理体*> obs;
	obs.reserve(num);
	for (uint32 i = 0; i < num; ++i) {
		S_物理几何体* geom = ((S_物理几何体*)boyd_ptr[i]->m_几何体);
		if (geom->m_Type == 几何类型) {
			obs.emplace_back(boyd_ptr[i]);
		}
	}
	return obs;
}

void f_PhyMujoco_构建内部几何体填充到物体(S_PhysicsMujoco* mj, std::vector<S_物理体*>& Body) {
	uint32 num = Body.size();
	auto boyd_ptr = Body.data();
	
	//for (uint32 i = 0; i < mj->m_模型->nbody; ++i) {
	for (uint32 i = 0; i < num; ++i) {
		S_物理体* body = boyd_ptr[i];
		if (!(body->m_绑定物体)) continue;

		//int32 几何体位置偏移 = mj->m_模型->body_geomadr[i];
		int32 几何体位置偏移 = boyd_ptr[i]->m_ID;

		//for (uint32 j = 0; j < mj->m_模型->body_geomnum[i]; ++j) {
		//int32 几何体位置偏移 = mj->m_模型->body_geomadr[i];
		//S_物理体* body = boyd_ptr[几何体位置偏移];
			
		switch (mj->m_模型->geom_type[几何体位置偏移]) {
		case mjtGeom::mjGEOM_PLANE:
			break;
		case mjtGeom::mjGEOM_SPHERE:
			break;
		case mjtGeom::mjGEOM_CAPSULE:break;
		case mjtGeom::mjGEOM_ELLIPSOID:break;
		case mjtGeom::mjGEOM_CYLINDER:break;
		case mjtGeom::mjGEOM_BOX:
			break;
		case mjtGeom::mjGEOM_MESH: {
			S_物体* 物体 = (S_物体*)body->m_绑定物体;
			
			int32 网格ID = mj->m_模型->geom_dataid[几何体位置偏移];

			uint32 vertOffset = mj->m_模型->mesh_vertadr[网格ID];
			uint32 IndexOffset = mj->m_模型->mesh_faceadr[网格ID];

			uint32 vertNum = mj->m_模型->mesh_vertnum[网格ID];
			uint32 faceNum = mj->m_模型->mesh_facenum[网格ID];

			std::vector<S_VNT1> vert;
			std::vector<uvec3> index;
			index.reserve(faceNum);
			for (uint32 k = 0; k < faceNum; ++k) {
				uvec3 face_index = ((uvec3*)mj->m_模型->mesh_face)[(k+IndexOffset)];

				S_VNT1 顶点0;
				顶点0.vert = ((vec3*)mj->m_模型->mesh_vert)[face_index.x + vertOffset];
				顶点0.normal = ((vec3*)mj->m_模型->mesh_normal)[face_index.x + vertOffset];
				顶点0.UV = {};
				S_VNT1 顶点1;
				顶点1.vert = ((vec3*)mj->m_模型->mesh_vert)[face_index.y + vertOffset];
				顶点1.normal = ((vec3*)mj->m_模型->mesh_normal)[face_index.y + vertOffset];
				顶点1.UV = {};
				S_VNT1 顶点2;
				顶点2.vert = ((vec3*)mj->m_模型->mesh_vert)[face_index.z + vertOffset];
				顶点2.normal = ((vec3*)mj->m_模型->mesh_normal)[face_index.z + vertOffset];
				顶点2.UV = {};

				vert.push_back(顶点0);
				vert.push_back(顶点1);
				vert.push_back(顶点2);
				index.push_back({k*3,k*3+1,k*3+2});
			}

			S_Mesh* me = f_ob_getMesh(物体, E_物体网格元素类型::e_MT_面);
			f_mesh_fill顶点数据(me, vert);
			//f_surface_fill索引数据(me, index);
			f_mesh_计算包围盒(me);
			//vec3 pos = ((vec3*)mj->m_模型->geom_pos)[几何体位置偏移];
			vec3 pos = ((vec3*)mj->m_模拟数据->geom_xpos)[几何体位置偏移];
			//vec3 pos = ((vec3*)mj->m_模型->body_pos)[i];
			int32 body链接ID = mj->m_模型->body_jntadr[几何体位置偏移];
			if (body链接ID >= 0) {
				pos = ((vec3*)mj->m_模型->jnt_pos)[body链接ID];
			}

			f_ob_setLoc((S_物体*)body->m_绑定物体, pos);
			//for (uint32 k = 0; k < indexNum; ++k) {
			//	std::cout << "mj->m_模型->mesh_face[IndexOffset] = " << mj->m_模型->mesh_face[IndexOffset + k] << std::endl;
			//}
			break;
		}
		}
		//}
	}
}

void f_PhyMujoco_施加力(S_PhysicsMujoco* mj, const S_施加物理力& 互交) {
	uint32 num = mj->m_Body.size();
	//auto boyd_ptr = mj->m_Body.data();


	// 施加直线力
	//double force[3] = { 100.0, 0.0, 0.0 };  // 设置直线力的大小和方向
	//mju_copy3(d->xfrc_applied + 6 * bodyId, force);  // 将直线力施加到刚体上

	// 施加关节力
	//double jointForce = 100.0;  // 设置关节力的大小
	//d->qfrc_applied[jointId] = jointForce;  // 将关节力施加到指定关节上

	// 施加扭矩
	//double torque[3] = { 0.0, 0.0, 10.0 };  // 设置扭矩的大小和方向
	//mju_copy3(d->xfrc_applied + 6 * bodyId, torque);  // 将扭矩施加到刚体上

	// 使用手动控制器控制关节位置
	//mjtNum targetPosition = 0.5;  // 设置目标关节位置
	//d->ctrl[jointId] = targetPosition;  // 将目标关节位置设置为手动控制器的控制信号



	mjtNum rpoint[6];
	//rpoint[0] = 受力点[0].x;
	//rpoint[1] = 受力点[0].y;
	//rpoint[2] = 受力点[0].z;
	rpoint[3] = 1;
	rpoint[4] = 1;
	rpoint[5] = 1;
	mjtNum dir[3];
	//dir[0] = 受力点[1].x;
	//dir[1] = 受力点[1].y;
	//dir[2] = 受力点[1].z;

	//for (uint32 i = 0; i < num; ++i) {
		//S_物理体* body = boyd_ptr[i];
		//mj->m_模型->nv;
		//mj_objectAcceleration(mj->m_模型, mj->m_模拟数据, mj->m_模型->site_size[body->m_ID], body->m_ID, rpoint, 1);
	//}

	mjvPerturb p = {};
	p.active = true;
	p.select = 互交.bodyID;
	//p.refpos[0] = 互交.force.x;
	//p.refpos[1] = 互交.force.y;
	//p.refpos[2] = 互交.force.z;
	p.scale = 1;
	p.localpos[0] = 0;
	p.localpos[1] = 0;
	p.localpos[2] = 0;

	p.refpos[0] = 互交.vec.x;
	p.refpos[1] = 互交.vec.y;
	p.refpos[2] = 互交.vec.z;

	//p.refpos[0] = 0;
	//p.refpos[1] = 1;
	//p.refpos[2] = 0;

	p.localpos[0] = 互交.pos.x;
	p.localpos[1] = 互交.pos.y;
	p.localpos[2] = 互交.pos.z;
	

	mjtNum qfrc_target = 10;
	//mj_applyFT(mj->m_模型, mj->m_模拟数据, p.refpos, rpoint, rpoint, 0, &qfrc_target);
	//mjv_applyPerturbForce(mj->m_模型, mj->m_模拟数据, &p);
	mjv_applyPerturbPose(mj->m_模型, mj->m_模拟数据, &p, 0);
}

void f_PhyMujoco_施加力(S_PhysicsMujoco* 解算器, const S_施加物理力& f, E_施加力类型 type) {
	switch (type) {
		case E_施加力类型::e_重力: {
			解算器->m_模型->opt.gravity[0] = f.force.x;
			解算器->m_模型->opt.gravity[1] = f.force.y;
			解算器->m_模型->opt.gravity[2] = f.force.z;
			//mj_setGravity(解算器->m_模型, );
			break;
		}
		case E_施加力类型::e_直线: {
			mjtNum 直线力[3] = { f.force.x, f.force.y, f.force.z };
			mjtNum 力施加点[3] = { f.pos.x, f.pos.y, f.pos.z };
			//mju_copy3(解算器->m_模拟数据->xfrc_applied + 6 * body_id, 直线力);
			mj_applyFT(解算器->m_模型, 解算器->m_模拟数据, 直线力, nullptr, 力施加点, f.bodyID, NULL);
			break;
		}
		case E_施加力类型::e_关节:
		case E_施加力类型::e_扭矩: {
			mjtNum 扭矩力[3] = { f.force.x, f.force.y, f.force.z };
			mjtNum 力施加点[3] = { f.pos.x, f.pos.y, f.pos.z };
			//mju_copy3(解算器->m_模拟数据->xfrc_applied + 6 * body_id, 扭矩力);
			mj_applyFT(解算器->m_模型, 解算器->m_模拟数据, nullptr, 扭矩力, 力施加点, f.bodyID, NULL);
			break;
		}
		//case E_施加力类型::e_关节: {
		//	解算器->m_模拟数据->ctrl[0] = 0;
		//	break;
		//}
		case E_施加力类型::e_控制: {
			
			break;
		}
	}
}

uint32 f_PhyMujoco_get肌腱数量(S_PhysicsMujoco* mj) {
	return mj->m_模型->ntendon;
}

void f_PhyMujoco_set肌腱力度(S_PhysicsMujoco* mj, float32* 力度, int32 i) {
	uint32 num = mj->m_模型->ntendon;

	//num = mj->m_模型->nu;
	/*for (uint32 i = 0; i < num; ++i) {
		auto a = mj->m_模型->actuator_trntype[i];
		auto b = mj->m_模型->actuator_dyntype[i];
		auto c = mj->m_模型->actuator_gaintype[i];
		auto d = mj->m_模型->actuator_trnid[i];
		auto e = mj->m_模型->actuator_lengthrange[i];
	}*/

	if (i >= 0 && i < num) {
		//int32 链接点数量 = mj->m_模拟数据->ten_wrapnum[i];
		//int32 链接点偏移 = mj->m_模拟数据->ten_wrapadr[i];
		//mj->m_模拟数据->ten_length[i] = 力度[0];
		//mj->m_模型->tendon_damping[i] = 力度[0];
		mj->m_模型->tendon_lengthspring[i] = 力度[0];
		//mj->m_模型->tendon_stiffness[i] = 力度[0];
		//mj->m_模型->tendon_invweight0[i] = 力度[0];
		//mj->m_模型->tendon_range[i] = 力度[0];
		//mj->m_模拟数据->ten_length[i] = 力度[0];
	}
	else {
		for (uint32 i = 0; i < num; ++i) {
			//mj->m_模拟数据->ten_length[i] = 力度[0];
			//mj->m_模型->tendon_damping[i] = 力度[0];
			mj->m_模型->tendon_lengthspring[i] = 力度[0];
			//mj->m_模型->tendon_stiffness[i] = 力度[0];
			//mj->m_模型->tendon_invweight0[i] = 力度[0];
			//mj->m_模型->tendon_range[i] = 力度[0];
			//mj->m_模拟数据->ten_length[i] = 力度[0];
		}
	}
}

void f_PhyMujoco_构建肌腱物体可视(S_PhysicsMujoco* mj, S_物体* Body) {
	
	int32 num = mj->m_模型->ntendon;

	auto mat = f_mat4_右手转左手坐标系(f_mat44_identity());

	for (uint32 i = 0; i < num; ++i) {
		uint32 tendon_num = mj->m_模型->tendon_num[i];

		int32 offset = mj->m_模型->tendon_adr[i];

		mjtObj obtype = (mjtObj)mj->m_模型->wrap_type[offset];
		int32 objid = mj->m_模型->wrap_objid[offset];


		S_Mesh* mesh = f_ob_getMesh(Body, i);
		

		int32 链接点数量 = mj->m_模拟数据->ten_wrapnum[i];
		int32 链接点偏移 = mj->m_模拟数据->ten_wrapadr[i];
		std::vector<S_VC> 控制点(链接点数量);
		for (int32 j = 0; j < 链接点数量; ++j) {
			int32 id = (j + 链接点偏移) * 3;
			S_VC point;
			point.vert.x = mj->m_模拟数据->wrap_xpos[id];
			point.vert.y = mj->m_模拟数据->wrap_xpos[id+1];
			point.vert.z = mj->m_模拟数据->wrap_xpos[id+2];
			point.vert = mat * point.vert;
			point.color = f_PackData4X8({ 200,60,10,255 });
			控制点[j] = point;
		}
		f_mesh_fill顶点数据(mesh, 控制点);

	}
}

void f_PhyMujoco_更新物体变换(S_PhysicsMujoco* 解算器) {
	uint32 body_num = 解算器->m_Body.size();
	auto body_ptr = 解算器->m_Body.data();

	for (uint32 ob_id = 0; ob_id < body_num; ++ob_id) {
		int32 几何体ID = body_ptr[ob_id]->m_ID;

		switch (body_ptr[ob_id]->m_物理体类型) {
			case E_物理体类型::E_物理体_静态: {
				continue;
			}
			default:
				break;
		}
		if (几何体ID == -1 || !(body_ptr[ob_id]->m_绑定物体)) continue;
		//--几何体ID;

		mat3X3 rot;
		vec3 pos;
		//vec4 q;
		for (uint8 i = 0; i < 9; ++i) {
			((float32*)&rot)[i] = 解算器->m_模拟数据->xmat[i + 几何体ID * 9];
		}
		for (uint8 i = 0; i < 3; ++i) {
			((float32*)&pos)[i] = 解算器->m_模拟数据->xpos[i + 几何体ID * 3];
		}
		//for (uint8 i = 0; i < 4; ++i) {
		//	((float32*)&q)[i] = 解算器->m_模拟数据->xquat[i + 几何体ID * 4];
		//}
		//f_mat4_构建缩放矩阵(body_ptr[ob_id]->m_变换.scale);
		

		S_物体* ob = (S_物体*)body_ptr[ob_id]->m_绑定物体;
		
		//rot = f_mat_3x3转置(rot);
		Mat44f mat = transformCoordinates(((float32(*)[3])&rot), (float32*)&pos);
		//Mat44f mat = f_mat44_构建移动矩阵(pos);
		
		static float32 a = 0;
		//Mat44f mat = f_mat33_to_44(rot);
		//mat *= f_mat4_构建缩放矩阵({-1,1,1});
		//mat = f_mat44_Inverse(mat);
		//mat = f_mat44_构建X旋转矩阵(90) * mat;
		auto s = f_mat4_构建缩放矩阵(ob->m_变换.scale);

		//switch (body_ptr[ob_id]->m_Type) {
		//	case E_物理体类型::E_物理体_刚体: {
		//		continue;
		//	}
		//	default:
		//		break;
		//}

		mat *= f_mat44_构建X旋转矩阵(-90);
		mat *= s;
		
		//if (body_ptr[ob_id]->m_虚拟链接轴绑定物体) {
		//	auto linkMat = f_mat44_构建XYZ变换矩阵(((S_物体*)body_ptr[ob_id]->m_虚拟链接轴绑定物体)->m_变换);
		//	linkMat *= mat;
		//
		//	f_ob_set矩阵(((S_物体*)body_ptr[ob_id]->m_虚拟链接轴绑定物体), linkMat);
		//}

		//mat *= f_mat44_构建Z旋转矩阵(45);
		//mat *= f_mat44_构建Z旋转矩阵(a);
		//a += 1;
		
		//mat = f_mat44_构建Z旋转矩阵(90) * mat;
		//mat *= f_mat44_构建Z旋转矩阵(90);
		//mat = f_mat44_构建(mat, pos);
		f_ob_set矩阵((S_物体*)body_ptr[ob_id]->m_绑定物体, mat);

		for (auto& e : ((S_物体*)body_ptr[ob_id]->m_绑定物体)->m_子物体) {
			auto ob = static_cast<S_物体*>(e);
			if (ob->m_Type == E_物体类型::t_网格物体 || ob->m_Type == E_物体类型::t_多边形) {
				f_ob_set矩阵(ob, mat);
			}
			else {
				ob->m_isUpdate = true;
			}
			//e->m_isUpdate = true;
		}
	}
}




/*int bodyid = mj_name2id(m, mjOBJ_BODY, "myfloatingbody");
int qposadr = -1, qveladr = -1;

// make sure we have a floating body: it has a single free joint
if (bodyid >= 0 && m->body_jntnum[bodyid] == 1 &&
	m->jnt_type[m->body_jntadr[bodyid]] == mjJNT_FREE)
{
	// extract the addresses from the joint specification
	qposadr = m->jnt_qposadr[m->body_jntadr[bodyid]];
	qveladr = m->jnt_dofadr[m->body_jntadr[bodyid]];
}*/

